import numpy as np
from Plantdata import *
import os
#from plot import *
import control as ct
from NDI import *
from utils import *
os.chdir(os.path.dirname(__file__))

longdata,ladata=read_data(r'data/longdata.csv',r'data/latdata.csv')
A_long,B_long,C_long,D_long,A_la,B_la,C_la,D_la,n_a,T,L_a,L_v,Y_b,v0,alpha0=get_ABCD(longdata,ladata,basicdata,mach=0.3)
m=basicdata['mass']
#print(B_long)

Plane_class=Plane(A_long,A_la,B_long,B_la,C_long,C_la,D_long,D_la)
Plane_model=Plane_class.getsys()

INDI_class=NDI_pqr(A_long,A_la,B_long,B_la,C_long,C_la,D_long,D_la)
INDI_model=INDI_class.getsys()

NDI_class=NDI_alphabetaphi(A_long,A_la,B_long,B_la,C_long,C_la,D_long,D_la)
NDI_model=NDI_class.getsys()

G_pqr=get_G_pqr(B_long,B_la)
G_alphabetaphi=get_G_alphabetaphi(A_long,A_la)

#print(Plane_model,INDI_model,NDI_model)
#print(G_pqr)
#print(G_alphabetaphi)


#期望动力学
#alpha纵向短周期
Kalpha_RQ=(1.7)**2
Kalpha_b=2*(0.85*1.7)
#beta荷兰滚
Kbeta_RQ=5**2
Kbeta_b=2*(5*0.9)
#phi滚转收敛
Kphi_p=10
#p
Kp_p=1
#q
Kp_q=1
#r
Kr_r=1

p_ideal = ct.TransferFunction([Kp_p], [1])
p_ideal=ct.tf2io(p_ideal,inputs="p",outputs='dp',name="p_ideal")
q_ideal = ct.TransferFunction([Kp_q],[1] )
q_ideal=ct.tf2io(q_ideal,inputs='q',outputs='dq',name="q_ideal")
r_ideal= ct.TransferFunction([Kr_r], [1])
r_ideal=ct.tf2io(r_ideal,inputs='r',outputs='dr',name="r_ideal")

alpha_ideal=ct.TransferFunction([Kalpha_RQ],[1,Kalpha_b])
alpha_ideal=ct.tf2io(alpha_ideal,inputs='alpha',outputs='dalpha',name="alpha_ideal")
beta_ideal=ct.TransferFunction([Kbeta_RQ],[1,Kbeta_b])
beta_ideal=ct.tf2io(beta_ideal,inputs='beta',outputs='dbeta',name="beta_ideal")
phi_ideal=ct.TransferFunction([Kphi_p],[1])
phi_ideal=ct.tf2io(phi_ideal,inputs='phi',outputs='dphi',name="phi_ideal")

#组装系统
System=ct.InterconnectedSystem(
    (Plane_model,INDI_model,NDI_model,p_ideal,q_ideal,r_ideal,alpha_ideal,beta_ideal,phi_ideal),
    connections=[
        #状态量

        ["NDI.v","Plane.v"],
        ["NDI.alpha","Plane.alpha"],
        ["NDI.q","Plane.q"],
        ["NDI.theta","Plane.theta"],
        ["NDI.beta","Plane.beta"],
        ["NDI.p","Plane.p"],
        ["NDI.r","Plane.r"],
        ["NDI.phi","Plane.phi"],

        ["NDI_pqr.v","Plane.v"],
        ["NDI_pqr.alpha","Plane.alpha"],
        ["NDI_pqr.q","Plane.q"],
        ["NDI_pqr.theta","Plane.theta"],
        ["NDI_pqr.beta","Plane.beta"],
        ["NDI_pqr.p","Plane.p"],
        ["NDI_pqr.r","Plane.r"],
        ["NDI_pqr.phi","Plane.phi"],

        ["NDI.de","Plane.de"],
        ["NDI.da","Plane.da"],
        ["NDI.dr","Plane.dr"],
        #控制信号
        ["Plane.de","NDI_pqr.de"],
        ["Plane.da","NDI_pqr.da"],
        ["Plane.dr","NDI_pqr.dr"],
        ["NDI_pqr.desire_p","p_ideal.dp"],
        ["NDI_pqr.desire_q","q_ideal.dq"],
        ["NDI_pqr.desire_r","r_ideal.dr"],

        ["p_ideal.p",'-Plane.p',"NDI.desire_p"],
        ["q_ideal.q",'-Plane.q',"NDI.desire_q"],
        ["r_ideal.r",'-Plane.r','NDI.desire_r'],

        ["alpha_ideal.alpha",'-Plane.alpha'],
        ["beta_ideal.beta",'-Plane.beta'],
        ["phi_ideal.phi",'-Plane.phi'],

        ["NDI.desire_alpha","alpha_ideal.dalpha"],
        ["NDI.desire_beta","beta_ideal.dbeta"],
        ["NDI.desire_phi","phi_ideal.dphi"],
    ],
    inplist=["alpha_ideal.alpha","beta_ideal.beta","phi_ideal.phi"],
    inputs=['desire_alpha', 'desire_beta', 'desire_phi'],
    outlist=["Plane.v","Plane.alpha","Plane.q","Plane.theta","Plane.beta","Plane.p","Plane.r","Plane.phi",
             'NDI.desire_p',"NDI.desire_q","NDI.desire_r",
             "NDI_pqr.de","NDI_pqr.da","NDI_pqr.dr",
             ],
    outputs=["v","alpha","q","theta","beta","p","r","phi","desire_p","desire_q","desire_r","de","da","dr"],
)
##开环系统
unct_System_long=ct.ss(A_long,B_long,C_long,D_long,inputs=["de"],outputs=["v","alpha","q","theta"],states=["v","alpha","q","theta"])
unct_System_la=ct.ss(A_la,B_la,C_la,D_la,inputs=["da","dr"],outputs=["beta","p","r","phi"],states=["beta","p","r","phi"])
#print(System)
#print(System.connection_table(show_names=1))

T = np.linspace(0, 50, 1000)



#这里响应序列的序号一定要匹配


#alpha
delta_desire_alpha = 0*np.ones(T.shape)
delta_desire_beta =0.0*np.ones(T.shape)
delta_desire_phi = 0.0*np.ones(T.shape)

X0=np.zeros((14,))
X0[1]=1
t,y = ct.input_output_response(
        System, T,
        #U
        [delta_desire_alpha, delta_desire_beta, delta_desire_phi], 
        #X0
        X0,
        #return_x=True,
        params={'G_pqr': G_pqr,"G_alphabetaphi":G_alphabetaphi,"T":T,"m":m,"L_a":L_a,"L_v":L_v,"Y_b":Y_b,"v0":v0,"alpha0":alpha0})
print("alpha")
#dlta_alpha冲击
t,y_long_open=ct.initial_response(unct_System_long,T,X0=[0,1,0,0],
                                  params={'G_pqr': G_pqr,"G_alphabetaphi":G_alphabetaphi,"T":T,"m":m,"L_a":L_a,"L_v":L_v,"Y_b":Y_b,"h":9000,"v0":v0,"alpha0":alpha0})
t,y_la_open=ct.initial_response(unct_System_la,T,X0=[0,0,0,0],
                                params={'G_pqr': G_pqr,"G_alphabetaphi":G_alphabetaphi,"T":T,"m":m,"L_a":L_a,"L_v":L_v,"Y_b":Y_b,"v0":v0,"alpha0":alpha0})      
#print(y_long_open.shape,y_la_open.shape)
fig1,ax1=plot_multiple_y(t,y[:8,:],y2s=np.concatenate((y_long_open,y_la_open),axis=0),plot_y2s=1,figname="alpha_res_states",
                         titles=["v","alpha","q","theta","beta","p","r","phi"])
#print(state)
fig,ax=plot_multiple_y(t,y[8:,:],figname="alpha_NDI_stepres_acu",
                       titles=["desired_p","desired_q","desired_r","de","da","dr"])
info=ct.step_info(y[1],T,SettlingTimeThreshold=0.05)
print(info)




#beta
delta_desire_alpha = 0*np.ones(T.shape)
delta_desire_beta =0*np.ones(T.shape)
delta_desire_phi = 0.0*np.ones(T.shape)

X0=np.zeros((14,))
X0[4]=1

t,y = ct.input_output_response(
        System, T,
        #U
        [delta_desire_alpha, delta_desire_beta, delta_desire_phi], 
        #X0
        X0,
        #return_x=True,
        params={'G_pqr': G_pqr,"G_alphabetaphi":G_alphabetaphi,"T":T,"m":m,"L_a":L_a,"L_v":L_v,"Y_b":Y_b})
print('beta')
#dlta_beta冲击
t,y_long_open=ct.initial_response(unct_System_long,T,X0=[0,0,0,0],
                                  params={'G_pqr': G_pqr,"G_alphabetaphi":G_alphabetaphi,"T":T,"m":m,"L_a":L_a,"L_v":L_v,"Y_b":Y_b})
t,y_la_open=ct.initial_response(unct_System_la,T,X0=[1,0,0,0],
                                params={'G_pqr': G_pqr,"G_alphabetaphi":G_alphabetaphi,"T":T,"m":m,"L_a":L_a,"L_v":L_v,"Y_b":Y_b})      
#print(y.shape)
#print(System.connection_table(show_names=1))
fig1,ax1=plot_multiple_y(t,y[:8,:],y2s=np.concatenate((y_long_open,y_la_open),axis=0),plot_y2s=1,figname="beta_res_states",
                         titles=["v","alpha","q","theta","beta","p","r","phi"])
#print(state)
fig,ax=plot_multiple_y(t,y[8:,:],figname="beta_stepres_acu",
                       titles=["desired_p","desired_q","desired_r","de","da","dr"])

info=ct.step_info(y[4],T,SettlingTimeThreshold=0.05)
print(info)



#phi
delta_desire_alpha = 0*np.ones(T.shape)
delta_desire_beta =0.0*np.ones(T.shape)
delta_desire_phi = 0*np.ones(T.shape)

X0=np.zeros((14,))
X0[7]=1

t,y = ct.input_output_response(
        System, T,
        #U
        [delta_desire_alpha, delta_desire_beta, delta_desire_phi], 
        #X0
        X0,
        #return_x=True,
        params={'G_pqr': G_pqr,"G_alphabetaphi":G_alphabetaphi,"T":T,"m":m,"L_a":L_a,"L_v":L_v,"Y_b":Y_b})
#dlta_phi冲击
t,y_long_open=ct.initial_response(unct_System_long,T,X0=[0,0,0,0],
                                  params={'G_pqr': G_pqr,"G_alphabetaphi":G_alphabetaphi,"T":T,"m":m,"L_a":L_a,"L_v":L_v,"Y_b":Y_b})
t,y_la_open=ct.initial_response(unct_System_la,T,X0=[0,0,0,1],
                                params={'G_pqr': G_pqr,"G_alphabetaphi":G_alphabetaphi,"T":T,"m":m,"L_a":L_a,"L_v":L_v,"Y_b":Y_b})      
print("phi")
#print(System.connection_table(show_names=1))
fig1,ax1=plot_multiple_y(t,y[:8,:],y2s=np.concatenate((y_long_open,y_la_open),axis=0),plot_y2s=1,figname="phi_res_states",
                         titles=["v","alpha","q","theta","beta","p","r","phi"])
#print(state)
fig,ax=plot_multiple_y(t,y[8:,:],figname="phi_stepres_acu",
                       titles=["desired_p","desired_q","desired_r","de","da","dr"])

info=ct.step_info(y[7],T,SettlingTimeThreshold=0.05)
print(info)
plt.show()


